#include <stdint.h>
#include "tim.h"
#include "oslib.h"
#include "ultrasonic.h"

#define CPU_FREQUENCY_MHZ 168
#define ULTRA_EVENT_DONE0 (1UL<<0)
#define ULTRA_EVENT_DONE1 (1UL<<1)


extern osEventFlagsId_t UltraEventHandle;

int32_t ultra0_cap_val;
int32_t ultra0_elp_cnt;
uint8_t ultra0_run;
uint8_t ultra0_rise;

int32_t ultra1_cap_val;
int32_t ultra1_elp_cnt;
uint8_t ultra1_run;
uint8_t ultra1_rise;

static void delay_us(uint32_t delay)
{
    int last, curr, val;
    int temp;
 
    while (delay != 0) {
        temp = delay > 900 ? 900 : delay;
        last = SysTick->VAL;
        curr = last - CPU_FREQUENCY_MHZ * temp;
        if (curr >= 0) {
            do {
                val = SysTick->VAL;
            } 
            while ((val < last) && (val >= curr));
        }
        else {
            curr += CPU_FREQUENCY_MHZ * 1000;
            do {
                val = SysTick->VAL;
            }
            while ((val <= last) || (val > curr));
        }
        delay -= temp;
    }
}

static void ultra_trigger(void *port, uint16_t pin, uint32_t us)
{
    HAL_GPIO_WritePin(port, pin, GPIO_PIN_SET);
    delay_us(20);
    HAL_GPIO_WritePin(port, pin, GPIO_PIN_RESET);
}

float ultra_read(uint8_t id)
{
    uint32_t evt = 0;
    if (id == 0) {
        ultra0_cap_val = 0;
        ultra0_elp_cnt = 0;
        ultra_trigger(TRIG0_GPIO_Port, TRIG0_Pin, 20);
        ultra0_run = 1;
        ultra0_rise = 0;
        evt = osEventFlagsWait(UltraEventHandle, ULTRA_EVENT_DONE0, osFlagsWaitAny, 10);
        if (evt == (uint32_t) osErrorTimeout) {
            return -1;
        } 
        else if (evt < osStatusReserved && (evt & ULTRA_EVENT_DONE0) == ULTRA_EVENT_DONE0) {
            return (float)(ultra0_cap_val + ultra0_elp_cnt * 65536) / 1e6 * 340 / 2.0 * 100;
        }
    } 
    else if (id == 1) {
        ultra1_cap_val = 0;
        ultra1_elp_cnt = 0;
        ultra_trigger(TRIG1_GPIO_Port, TRIG1_Pin, 20);
        ultra1_run = 1;
        ultra1_rise = 0;
        evt = osEventFlagsWait(UltraEventHandle, ULTRA_EVENT_DONE1, osFlagsWaitAny, 10);
        if (evt == (uint32_t) osErrorTimeout) {
            return -1;
        } 
        else if (evt < osStatusReserved && (evt & ULTRA_EVENT_DONE1) == ULTRA_EVENT_DONE1) {
            return (float)(ultra1_cap_val + ultra1_elp_cnt * 65536) / 1e6 * 340 / 2.0 * 100;
        }
        else {
            uprintf("evt = %x\n", evt);
        }
    }
    else {
        uprintf("[Error] Wrong ultra number!\n");
    }
    return -2;
}

void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim)
{
    if (htim->Instance == TIM9) {
        if (htim->Channel == HAL_TIM_ACTIVE_CHANNEL_1) {
            if (ultra0_run) {
                if (!ultra0_rise) {
                    ultra0_rise = 1;
                    ultra0_elp_cnt = 0;
                    ultra0_cap_val = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_1);
                    TIM_RESET_CAPTUREPOLARITY(htim, TIM_CHANNEL_1);
                    TIM_SET_CAPTUREPOLARITY(htim, TIM_CHANNEL_1, TIM_ICPOLARITY_FALLING);
                }
                else {
                    ultra0_run = 0;
                    ultra0_rise = 0;
                    ultra0_cap_val = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_1) - ultra0_cap_val;
                    TIM_RESET_CAPTUREPOLARITY(htim, TIM_CHANNEL_1);
                    TIM_SET_CAPTUREPOLARITY(htim, TIM_CHANNEL_1, TIM_ICPOLARITY_RISING); 
                    // TODO: 释放信号量, 采集完成
                    osEventFlagsSet(UltraEventHandle, ULTRA_EVENT_DONE0);
                }
            }
        }
        else if (htim->Channel == HAL_TIM_ACTIVE_CHANNEL_2) {
            if (ultra1_run) {
                if (!ultra1_rise) {
                    ultra1_rise = 1;
                    ultra1_elp_cnt = 0;
                    ultra1_cap_val = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_2);
                    TIM_RESET_CAPTUREPOLARITY(htim, TIM_CHANNEL_2);
                    TIM_SET_CAPTUREPOLARITY(htim, TIM_CHANNEL_2, TIM_ICPOLARITY_FALLING);
                }
                else {
                    ultra1_run = 0;
                    ultra1_rise = 0;
                    ultra1_cap_val = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_2) - ultra1_cap_val;
                    TIM_RESET_CAPTUREPOLARITY(htim, TIM_CHANNEL_2);
                    TIM_SET_CAPTUREPOLARITY(htim, TIM_CHANNEL_2, TIM_ICPOLARITY_RISING); 
                    // TODO: 释放信号量, 采集完成
                    osEventFlagsSet(UltraEventHandle, ULTRA_EVENT_DONE1);
                }
            }
        }
    }
}

/* 移动到main.c中 */
// void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
// {
//     if (htim->Instance == TIM9) {
//         if (htim->Channel == HAL_TIM_ACTIVE_CHANNEL_1) {
//             if (ultra0_run && ultra0_rise) {
//                 ultra0_elp_cnt++;
//             }
//         }
//         else if (htim->Channel == HAL_TIM_ACTIVE_CHANNEL_2) {
//             if (ultra1_run && ultra1_rise) {
//                 ultra1_elp_cnt++;
//             }
//         }
//     }
// }
